#include "StdAfx.h"
#include "ImaAdpcm.h"

/* Intel ADPCM step variation table */
static int indexTable[16] = {
	-1, -1, -1, -1, 2, 4, 6, 8,
	-1, -1, -1, -1, 2, 4, 6, 8,
};

static int stepsizeTable[89] = {
	7, 8, 9, 10, 11, 12, 13, 14, 16, 17,
	19, 21, 23, 25, 28, 31, 34, 37, 41, 45,
	50, 55, 60, 66, 73, 80, 88, 97, 107, 118,
	130, 143, 157, 173, 190, 209, 230, 253, 279, 307,
	337, 371, 408, 449, 494, 544, 598, 658, 724, 796,
	876, 963, 1060, 1166, 1282, 1411, 1552, 1707, 1878, 2066,
	2272, 2499, 2749, 3024, 3327, 3660, 4026, 4428, 4871, 5358,
	5894, 6484, 7132, 7845, 8630, 9493, 10442, 11487, 12635, 13899,
	15289, 16818, 18500, 20350, 22385, 24623, 27086, 29794, 32767
};

ImaAdpcm::ImaAdpcm(void)
{
}

ImaAdpcm::~ImaAdpcm(void)
{
}

void ImaAdpcm::adpcm_decoder(char *indata, char* outdata, int len, int &nOutLen)
{
	char  *inp;                 /* Input buffer pointer */
	short *outp;                /* output buffer pointer */
	int sign;                   /* Current adpcm sign bit */
	int delta;                  /* Current adpcm output value */
	int step;                   /* Stepsize */
	int valpred;                /* Predicted value */
	int vpdiff;                 /* Current change to valpred */
	int index;                  /* Current step change index */
	int inputbuffer;            /* place to keep next 4-bit value */
	int bufferstep;             /* toggle between inputbuffer/input */

	int nSize = 0;
	int nLoop = len/128;

	outp    = (short*)outdata;
	nOutLen = 0;

	for( int k=0;k<nLoop; k++ )
	{
		inp  = indata + k*128;

		valpred = (int)* ( (short*)inp );
		inp += 2;
		index   = (int)* ( (char*)inp );
		inp += 2;
		step    = stepsizeTable[index];

		bufferstep = 0;

		*outp++ = valpred;
		nOutLen += 2;

		nSize = 248;

		for ( ; nSize > 0 ; nSize-- )  
		{
			/* Step 1 - get the delta value */
			if ( bufferstep ) 
			{
				delta = (inputbuffer >> 4) & 0xf;
				//delta = inputbuffer & 0xf;
			} 
			else {
				inputbuffer = *inp++;
				delta = inputbuffer & 0xf;
				//delta = (inputbuffer >> 4) & 0xf;
			}
			bufferstep = !bufferstep;

			/* Step 2 - Find new index value (for later) */
			index += indexTable[delta];
			if ( index < 0 ) index = 0;
			if ( index > 88 ) index = 88;

			/* Step 3 - Separate sign and magnitude */
			sign  = delta & 8;
			delta = delta & 7;

			/* Step 4 - Compute difference and new predicted value */
			/*
			** Computes 'vpdiff = (delta+0.5)*step/4', but see comment
			** in adpcm_coder.
			*/
			vpdiff = step >> 3;
			if ( delta & 4 ) vpdiff += step;
			if ( delta & 2 ) vpdiff += step>>1;
			if ( delta & 1 ) vpdiff += step>>2;

			if ( sign )
				valpred -= vpdiff;
			else
				valpred += vpdiff;

			/* Step 5 - clamp output value */
			if ( valpred > 32767 )
				valpred = 32767;
			else if ( valpred < -32768 )
				valpred = -32768;

			/* Step 6 - Update step value */
			step = stepsizeTable[index];

			/* Step 7 - Output value */
			*outp++ = valpred;
			nOutLen += 2;
		}
	}
}

void ImaAdpcm::adpcm_encoder(char *indata, char* outdata, int len, int &nOutLen)
{
	short *inp;                 /* Input buffer pointer */
	char  *outp;                /* output buffer pointer */
	long val;                   /* Current input sample value */
	long sign;                  /* Current adpcm sign bit */
	long delta;                 /* Current adpcm output value */
	long step;                  /* Stepsize */
	long valprev;               /* virtual previous output value */
	long vpdiff;                /* Current change to valprev */
	long index;                 /* Current step change index */
	long outputbuffer;          /* place to keep previous 4-bit value */
	long bufferstep;            /* toggle between outputbuffer/output */

	outp = outdata;

	int nSize = 0;
	int nLoop = len/498;

	outp    = outdata;
	nOutLen = 0;

	for( int k=0;k<nLoop; k++ )
	{
		inp  = (short*) ( indata + k*498 );

		index = 0;
		step  = stepsizeTable[index];

		/*save the 1st sample in 16 bits*/
		val = *inp++;
		bufferstep = 1;
		*((short *)outp)= val;
		outp    += 2;
		nOutLen += 2;

		/*Save initial index in 16 bits*/
		*((short *)outp)= (short)index;
		outp    += 2;
		nOutLen += 2;

		valprev = val;

		nSize = 248;

		for ( ; nSize > 0 ; nSize-- ) 
		{
			val = *inp++;

			/* Step 1 - compute difference with previous value */
			delta = val - valprev;
			sign = (delta < 0) ? 8 : 0;
			if ( sign ) delta = (-delta);

			/* Step 2 - Divide and clamp */
			{
				long tmp = 0;

				vpdiff = step/8;
				if ( delta >= step ) 
				{
					tmp = 4;
					delta -= step;
					vpdiff += step;
				}
				step >>= 1;
				if ( delta >= step  ) 
				{
					tmp |= 2;
					delta -= step;
					vpdiff += step;
				}
				step >>= 1;
				if ( delta >= step ) 
				{
					tmp |= 1;
					vpdiff += step;
				}
				delta = tmp;
			}

			/* Step 3 - Update previous value */
			if ( sign )
				valprev -= vpdiff;
			else
				valprev += vpdiff;

			/* Step 4 - Clamp previous value to 16 bits */
			if ( valprev > 32767 )
				valprev = 32767;
			else if ( valprev < -32768 )
				valprev = -32768;

			/* Step 5 - Assemble value, update index and step values */
			delta |= sign;

			index += indexTable[delta];
			if ( index < 0 ) index = 0;
			if ( index > 88 ) index = 88;
			step = stepsizeTable[index];

			/* Step 6 - Output value */
			if ( bufferstep ) 
			{
				outputbuffer = (delta & 0x0f);
			}
			else 
			{
				*outp++ = ((delta << 4) & 0xf0) | outputbuffer;
				nOutLen ++;
			}
			bufferstep = !bufferstep;
		}

		/* Output last step, if needed */
		if ( !bufferstep )
		{
			*outp++ = outputbuffer;
			nOutLen ++;
		}
	}
}

